Synchronous Maneuver Searching and Trajectory Planning for Autonomous Vehicles in Dynamic Traffic Environments

In the real-time decision-making and local planning process of autonomous vehicles in dynamic environments, driving system may fail to find a reasonable policy or even gets trapped some situation due complexity global tasks incompatibility between upper level maneuver decisions with lower trajectory planning. To solve this problem, paper presents synchronous searching (SMSTP) algorithm based on topological concept homotopy. Firstly, set alternative maneuvers boundary limits are enumerated multi-lane road. Instead sampling numerous paths whole spatio-temporal space, we, for first time, propose using Trajectory Profiles (TPs) quickly construct represented by different routes, put forward corridor generation graph-search. The bounded further constrains maneuver’s space spatial space. A step-wise heuristic optimization is then proposed synchronously generate feasible each maneuver. achieve performance, we initialize states be optimized constraints maneuvers, as terminal targets quadratic cost function. solution always guaranteed only if specific given. simulation realistic driving-test experiments verified that SMSTP has short computation time which less than 37 ms, experimental results showed validity effectiveness algorithm.